#ifndef TRANSFORMATIONS_H
#define TRANSFORMATIONS_H
//------------------------------------------------------------------------
//
//  Name:   Transformations.h
//
//  Desc:   Functions for converting 2D vectors between World and Local
//          space.
//------------------------------------------------------------------------
#include <vector>
#include "Vector2D.h"
#include "Matrix2D.h"






//--------------------------- WorldTransform -----------------------------
//
//  given a std::vector of 2D vectors, a position, orientation and scale,
//  this function transforms the 2D vectors into the object's world space
//------------------------------------------------------------------------
inline std::vector<Vector2D> WorldTransform(std::vector<Vector2D> &points,
                                            const Vector2D   &pos,
                                            const Vector2D   &forward,
                                            const Vector2D   &side,
                                            const Vector2D   &scale)
{
    //copy the original vertices into the buffer about to be transformed
    std::vector<Vector2D> TranVector2Ds = points;

    //create a transformation matrix
    Matrix2D matTransform;

    //scale
    if ( (scale.x != 1.0) || (scale.y != 1.0) )
    {
        matTransform.Scale(scale.x, scale.y);
    }

    //rotate
    matTransform.Rotate(forward, side);

    //and translate
    matTransform.Translate(pos.x, pos.y);

    //now transform the object's vertices
    matTransform.TransformVector2Ds(TranVector2Ds);

    return TranVector2Ds;
}

//--------------------------- WorldTransform -----------------------------
//
//  given a std::vector of 2D vectors, a position and  orientation
//  this function transforms the 2D vectors into the object's world space
//------------------------------------------------------------------------
inline std::vector<Vector2D> WorldTransform(std::vector<Vector2D> &points,
                                 const Vector2D   &pos,
                                 const Vector2D   &forward,
                                 const Vector2D   &side)
{
    //copy the original vertices into the buffer about to be transformed
    std::vector<Vector2D> TranVector2Ds = points;

    //create a transformation matrix
    Matrix2D matTransform;

    //rotate
    matTransform.Rotate(forward, side);

    //and translate
    matTransform.Translate(pos.x, pos.y);

    //now transform the object's vertices
    matTransform.TransformVector2Ds(TranVector2Ds);

    return TranVector2Ds;
}

//--------------------- PointToWorldSpace --------------------------------
//
//  Transforms a point from the agent's local space into world space
//------------------------------------------------------------------------
inline Vector2D PointToWorldSpace(const Vector2D &point,
                                    const Vector2D &AgentHeading,
                                    const Vector2D &AgentSide,
                                    const Vector2D &AgentPosition)
{
    //make a copy of the point
    Vector2D TransPoint = point;

    //create a transformation matrix
    Matrix2D matTransform;

    //rotate
    matTransform.Rotate(AgentHeading, AgentSide);

    //and translate
    matTransform.Translate(AgentPosition.x, AgentPosition.y);

    //now transform the vertices
    matTransform.TransformVector2Ds(TransPoint);

    return TransPoint;
}



//--------------------- PointToLocalSpace --------------------------------
//
//------------------------------------------------------------------------
inline Vector2D PointToLocalSpace(const Vector2D &point,
                             Vector2D &AgentHeading,
                             Vector2D &AgentSide,
                              Vector2D &AgentPosition)
{

    //make a copy of the point
    Vector2D TransPoint = point;

    //create a transformation matrix
    Matrix2D matTransform;

    float Tx = -AgentPosition.dot(AgentHeading);
    float Ty = -AgentPosition.dot(AgentSide);

    //create the transformation matrix
    matTransform._11(AgentHeading.x); matTransform._12(AgentSide.x);
    matTransform._21(AgentHeading.y); matTransform._22(AgentSide.y);
    matTransform._31(Tx);           matTransform._32(Ty);

    //now transform the vertices
    matTransform.TransformVector2Ds(TransPoint);

    return TransPoint;
}


//--------------------- VectorToWorldSpace --------------------------------
//
//  Transforms a vector from the agent's local space into world space
//------------------------------------------------------------------------
inline Vector2D VectorToWorldSpace(const Vector2D &vec,
                                     const Vector2D &AgentHeading,
                                     const Vector2D &AgentSide)
{
    //make a copy of the point
    Vector2D TransVec = vec;

    //create a transformation matrix
    Matrix2D matTransform;

    //rotate
    matTransform.Rotate(AgentHeading, AgentSide);

    //now transform the vertices
    matTransform.TransformVector2Ds(TransVec);

    return TransVec;
}

//--------------------- VectorToLocalSpace --------------------------------
//
//------------------------------------------------------------------------
inline Vector2D VectorToLocalSpace(const Vector2D &vec,
                             const Vector2D &AgentHeading,
                             const Vector2D &AgentSide)
{ 
    
    //make a copy of the point
    Vector2D TransPoint = vec;

    //create a transformation matrix
    Matrix2D matTransform;

    //create the transformation matrix
    matTransform._11(AgentHeading.x); matTransform._12(AgentSide.x);
    matTransform._21(AgentHeading.y); matTransform._22(AgentSide.y);

    //now transform the vertices
    matTransform.TransformVector2Ds(TransPoint);

    return TransPoint;
}


//-------------------------- Vec2DRotateAroundOrigin --------------------------
//
//  rotates a vector ang rads around the origin
//-----------------------------------------------------------------------------
inline void Vec2DRotateAroundOrigin(Vector2D& v, float ang)
{
    //create a transformation matrix
    Matrix2D mat;

    //rotate
    mat.Rotate(ang);

    //now transform the object's vertices
    mat.TransformVector2Ds(v);
}

//------------------------ CreateWhiskers ------------------------------------
//
//  given an origin, a facing direction, a 'field of view' describing the 
//  limit of the outer whiskers, a whisker length and the number of whiskers
//  this method returns a vector containing the end positions of a series
//  of whiskers radiating away from the origin and with equal distance between
//  them. (like the spokes of a wheel clipped to a specific segment size)
//----------------------------------------------------------------------------
inline std::vector<Vector2D> CreateWhiskers(unsigned int  NumWhiskers,
                                            float        WhiskerLength,
                                            float        fov,
                                            Vector2D      facing,
                                            Vector2D      origin)
{
    //this is the magnitude of the angle separating each whisker
    float SectorSize = fov/(float)(NumWhiskers-1);

    std::vector<Vector2D> whiskers;
    Vector2D temp;
    float angle = -fov*0.5; 

    for (unsigned int w=0; w<NumWhiskers; ++w)
    {
        //create the whisker extending outwards at this angle
        temp = facing;
        Vec2DRotateAroundOrigin(temp, angle);
        whiskers.push_back(origin + temp * WhiskerLength);

        angle += SectorSize;
    }
    
    return whiskers;
}


#endif
